#ifndef NCCTRACKER_H_
#define NCCTRACKER_H_

#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include <vector>

using namespace cv;
using namespace Eigen;

class Camera;

/*
 * reference shelley msckf p41
 */
class NccTracker
{
public:
    NccTracker();
    NccTracker(int patch_size, Matrix3d& prev_R_CG, Matrix3d& cur_R_CG, Vector3d& trans);
    ~NccTracker();

    /*
     * set the global gravirity value
     */
    inline void setGravirity(Vector3d& gravirity);

    /* track the image features
        input: prev_img: previous image
               cur_img: current image
               prev_features: previous feature coordinates
               cur_features: current feature coordinats
               track_status: tracking status, true for ok, false for lost
    */
    void Track(Mat& prev_img, Mat& cur_img, vector<Vector2d>& prev_features,
               vector<Vector2d>& cur_features, vector<bool>& track_status);

protected:
    // extract the patch around the feature
    Mat extractPatch(Mat& img, Vector2d& feature, Matrix3d& rot_mat);

    // normalize the patch
    void normalize(Mat& patch);

    // search the epipolar line to get the most likely image patch
    void searchEpipolarLine(Mat& img_patch, Mat& dst_img, Vector2d& cur_ft);

private:
    // forward and backward tracking tricks
    bool use_fb_;
    // if normalize the image
    bool normalize_;
    // the patch is
    int patch_size_;
    // previous frame orientation matrix
    Matrix3d prev_R_CG_;

    // current frame orientation matrix
    Matrix3d cur_R_CG_;

    // translation from previous frame to current frame
    Vector3d t_C1C2_;

    // global gravirity value
    Vector3d gravirity_;
    // camera
    Camera* camera_ptr_;
    // image rotated to gravirity orientation
    Mat rotated_img_;
};

#endif
